#include <aversive.h>
#include <aversive/wait.h>
#include <uart.h>
#include <stdio.h>
#include <stdlib.h>
#include <scheduler.h>

#include "compass.h"
#include "hcontrol_system.h"
#include "hrobot_system.h"
#include "hposition_manager.h"
#include "comm_fpga.h"

#include <avr/sfr_defs.h>

hcontrol_system hcs;
hposition_manager hpm;
hrobot_system hrs;

void update_position(void*);

// 
int main(void)
{
  // Turn interruptions ON
  sei();

  // Initialize UART
  uart_init();
  fdevopen(uart1_dev_send, uart1_dev_recv);

  // Some advertisment :p
  printf("\n\nRobotter 2008 - 102chute - UNIOC HOLONOMIC PROPULSION\n");
  printf("Compiled "__DATE__" at "__TIME__"\n");
  printf("\nBooting...\n");

  // Initialize FPGA communication
  commfpga_init();

  // Initialize hrobot system
  hrs_init(&hrs);

  hrs_set_CO1_encoder(&hrs, commfpga_get_CO1, NULL);
  hrs_set_CO2_encoder(&hrs, commfpga_get_CO2, NULL);
  hrs_set_CO3_encoder(&hrs, commfpga_get_CO3, NULL);
  
  hrs_set_M1_motor(&hrs, commfpga_set_M1, NULL);
  hrs_set_M2_motor(&hrs, commfpga_set_M2, NULL);
  hrs_set_M3_motor(&hrs, commfpga_set_M3, NULL);

  hrs_set_motors_signs(&hrs, 1, -1, 1);

  // Initialize hposition manager
  hpm_init(&hpm);
 
  hpm_set_related_hrobot_system(&hpm,&hrs);
  hpm_set_physical_parameters(&hpm,10.0);

  // Initialize hcontrol system
  hcs_init(&hcs);

  hcs_set_related_hrobot_system(&hcs,&hrs);
  hcs_set_related_hposition_manager(&hcs,&hpm);

  // Set up electronic compass
  compass_init();

  // Set all PWMs to zero
  hrs_set_M1(&hrs,0);
  hrs_set_M2(&hrs,0);
  hrs_set_M3(&hrs,0);

  // Remove parking break
  sbi(DDRB,7);
  sbi(PORTB,7);

  // Initialize scheduler and start position & control system tasks
  scheduler_init();
 // scheduler_add_periodical_event_priority(&update_position, NULL, 200, 70);

  // Ready up
  printf("done.\n\n");

  uint16_t start_bearing,bearing;
  uint16_t cor;

  start_bearing = compass_getBearing_deg();
  
  printf("start_bearing = %d\n\n",start_bearing);

  while(1)
  {
    bearing = compass_getBearing_deg();
    
    cor = 1*( start_bearing - bearing );

    commfpga_set_M1(NULL,-600+cor);
    commfpga_set_M2(NULL,-600+cor);
    commfpga_set_M3(NULL,   0+cor);
    
    printf("\rbearing = %u diff = %u cor = %u          ",start_bearing - bearing,bearing,cor);
    wait_ms(50);
  }

  while(1) nop();
  return 0;
}

//************************

void update_position(void* dummy)
{
  // DEBUG: Light up led

  // Update sensors data
  hrs_update(&hrs);

  // Update position
  hpm_update(&hpm);

}
